#include "p24F04KA200.h"
#include "uart.h"
#include "util.h"

void writeStr(char *str) {
    while (*str) {
        write((unsigned char) *str);
        str++;
    }
}

void writeStrManchester(char *str) {
    while (*str) {
        writeManchester((unsigned char) *str);
        str++;
    }
}

void write(unsigned int data) {
    while (U1STAbits.UTXBF);
    U1TXREG = data;
}

unsigned char writeManchesterAux(unsigned char data) {
    unsigned char word = 0;
    if (data & 0b1000) {
        word = 0b10;
    } else {
        word = 0b01;
    }
    if (data & 0b0100) {
        word = (word << 2) | 0b10;
    } else {
        word = (word << 2) | 0b01;
    }
    if (data & 0b0010) {
        word = (word << 2) | 0b10;
    } else {
        word = (word << 2) | 0b01;
    }
    if (data & 0b0001) {
        word = (word << 2) | 0b10;
    } else {
        word = (word << 2) | 0b01;
    }
    return word;
}

/*
 * 01101001
 * 0110 - 1001
 * 01 10 10 01 - 10 01 01 10
 */
void writeManchester(unsigned char data) {
    write(writeManchesterAux(data >> 4));
    write(writeManchesterAux(data));
}

unsigned int readManchesterAux(unsigned int word) {
    unsigned int data = 0;
    if (word & 0b10000000) {
        data = data | 0b1000;
    }
    if (word & 0b00100000) {
        data = data | 0b0100;
    }
    if (word & 0b00001000) {
        data = data | 0b0010;
    }
    if (word & 0b00000010) {
        data = data | 0b0001;
    }
    return data;
}

unsigned int readManchester() {
    unsigned int w1 = read();
    unsigned int w2 = read();
    unsigned int data = 0;

    data = readManchesterAux(w1) << 4;
    data = data | readManchesterAux(w2);
    return data;
}

void writeStrUnsLn(char *str, unsigned int i) {
    writeStr(str);
    writeStr(itoa(i, 10));
    writeStr("\r\n");
}

unsigned int read() {
    while (!U1STAbits.URXDA);
    U1STAbits.OERR = 0;
    return U1RXREG;
}

void uart_init(unsigned baud, unsigned BRGH, unsigned RXINV, unsigned UTXINV) {

    // configure U2MODE
    U1MODEbits.UARTEN = 0; // Bit15 TX, RX DISABLED, ENABLE at end of func
    U1MODEbits.USIDL = 0; // Bit13 Continue in Idle
    U1MODEbits.IREN = 0; // Bit12 No IR translation
    U1MODEbits.RTSMD = 0; // Bit11 Simplex Mode
    U1MODEbits.UEN = 0; // Bits8,9 TX,RX enabled, CTS,RTS not
    U1MODEbits.WAKE = 0; // Bit7 No Wake up (since we don't sleep here)
    U1MODEbits.LPBACK = 0; // Bit6 No Loop Back
    U1MODEbits.ABAUD = 0; // Bit5 No Autobaud (would require sending '55')
    U1MODEbits.RXINV = 0; // Bit4 IdleState = 1
    U1MODEbits.BRGH = 0; // Bit3 16 clocks per bit period
    U1MODEbits.PDSEL = 0; // Bits1,2 8bit, No Parity
    U1MODEbits.STSEL = 0; // Bit0 One Stop Bit

    U1MODEbits.BRGH = BRGH; // Bit3 16 clocks per bit period
    U1BRG = baud;

    // Load all values in for U1STA SFR
    U1STAbits.UTXISEL1 = 0; //Bit15 Int when Char is transferred (1/2 config!)
    U1STAbits.UTXINV = 1; //Bit14 N/A, IRDA config
    U1STAbits.UTXISEL0 = 0; //Bit13 Other half of Bit15
    U1STAbits.UTXBRK = 0; //Bit11 Disabled
    U1STAbits.UTXEN = 0; //Bit10 TX pins controlled by periph
    U1STAbits.UTXBF = 0; //Bit9 *Read Only Bit*
    U1STAbits.TRMT = 0; //Bit8 *Read Only bit*
    U1STAbits.URXISEL = 0; //Bits6,7 Int. on character recieved
    U1STAbits.ADDEN = 0; //Bit5 Address Detect Disabled
    U1STAbits.RIDLE = 0; //Bit4 *Read Only Bit*
    U1STAbits.PERR = 0; //Bit3 *Read Only Bit*
    U1STAbits.FERR = 0; //Bit2 *Read Only Bit*
    U1STAbits.OERR = 0; //Bit1 *Read Only Bit*
    U1STAbits.URXDA = 0; //Bit0 *Read Only Bit*

    IFS0bits.U1TXIF = 0; // Clear the Transmit Interrupt Flag
    IEC0bits.U1TXIE = 0; // Enable Transmit Interrupts
    IFS0bits.U1RXIF = 0; // Clear the Recieve Interrupt Flag
    IEC0bits.U1RXIE = 0; // Enable Recieve Interrupts

    U1MODEbits.RXINV = RXINV; // Bit4 IdleState = 1
    U1STAbits.UTXINV = UTXINV; //Bit14 N/A, IRDA config

    U1MODEbits.UARTEN = 1; // And turn the peripheral on

    U1STAbits.UTXEN = 1;
}
